#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "bzrc.h"

#define IP_ADDRESS "127.0.0.1"
#define FLAG_RADIUS 3.0f

#define FLAG_STEP 1.0f // this is alpha
#define OBSTACLE_STEP 0.08f // this is beta
#define TANGENT_STEP 0.001f // beta, but for tangent field

#define FLAG_SPREAD 20.0f
#define OBSTACLE_SPREAD 140.0f
#define TANGENT_SPREAD 0.1f
#define F_INFINITY 15
extern int debug;

int plot();
void printObstacles(FILE* fSocket, FILE* plotFile);
void printFields(FILE* fSocket, FILE* plotFile);
pair avoidObstacle(float x, float y, float xo, float yo, float radiuso);
pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed);
pair circleObstacle(float x, float y, float xo, float yo, float radiusObs);
float getAngleBetween(float x, float y, float xg, float yg);
float getDistanceBetween(float x, float y, float xg, float yg);
int insideObstacle(float x, float y, obstacle* obstacles, int obstacleCount);

int main(int argc, char* argv[]) {
	int tcpSocket = 0;
	FILE* fSocket = 0;
	FILE* plotFile;
	int i;
	if (argc < 2) {
		fprintf(stderr, "usage: plot port [debug 1:0]\n");
		exit(-1);
	}
	if (argc == 3 && atoi(argv[2]) == 1) {
		debug = 1;
	}
	BZRCConnect(IP_ADDRESS, atoi(argv[1]), &tcpSocket, &fSocket);
	plotFile = fopen("plot.gpi", "w");
	fprintf(plotFile, "set title \"My Plot\"\n");
	fprintf(plotFile, "set xrange [-400.0: 400.0]\n");
	fprintf(plotFile, "set yrange [-400.0: 400.0]\n");
	fprintf(plotFile, "unset key\n");
	//fprintf(plotFile, "set term svg\n");
	//fprintf(plotFile, "set output 'plot.svg'\n");
	printObstacles(fSocket, plotFile);
	fprintf(plotFile, "plot '-' with vectors head\n");
	printFields(fSocket, plotFile);
	fprintf(plotFile, "e\n");
	fclose(plotFile);
	BZRCDisconnect(tcpSocket, fSocket);
	plot();
	return 0;
}

void printObstacles(FILE* fSocket, FILE* plotFile) {
	obstacle* obstacles;
	int obstacleCount;
	int i;
	getObstacles(fSocket, &obstacles, &obstacleCount);
	for (i = 0; i < obstacleCount; i++) {
		fprintf(plotFile, "set arrow from %02.1f, %02.1f to %02.1f, %02.1f nohead lt 3\n",
			obstacles[i].corners[0].x,
			obstacles[i].corners[0].y,
			obstacles[i].corners[1].x,
			obstacles[i].corners[1].y
		);
		fprintf(plotFile, "set arrow from %02.1f, %02.1f to %02.1f, %02.1f nohead lt 3\n",
			obstacles[i].corners[1].x,
			obstacles[i].corners[1].y,
			obstacles[i].corners[2].x,
			obstacles[i].corners[2].y
		);
		fprintf(plotFile, "set arrow from %02.1f, %02.1f to %02.1f, %02.1f nohead lt 3\n",
			obstacles[i].corners[2].x,
			obstacles[i].corners[2].y,
			obstacles[i].corners[3].x,
			obstacles[i].corners[3].y
		);
		fprintf(plotFile, "set arrow from %02.1f, %02.1f to %02.1f, %02.1f nohead lt 3\n",
			obstacles[i].corners[3].x,
			obstacles[i].corners[3].y,
			obstacles[i].corners[0].x,
			obstacles[i].corners[0].y
		);

	}
	free(obstacles);
	return;
}

void printFields(FILE* fSocket, FILE* plotFile) {
	int i;
	int j;
	int oIdx;
	obstacle* obstacles;
	int obstacleCount;
	flag* flags;
	int flagCount;
	base* bases;
	int baseCount;
	float dumpySpeed;
	pair delta;
	pair tempDelta;
	delta.x = 0;
	delta.y = 0;
	getObstacles(fSocket, &obstacles, &obstacleCount);
	getFlags(fSocket, &flags, &flagCount);
	getBases(fSocket, &bases, &baseCount);
	for (i = -400; i < 400; i = i + 15) {
		for (j = -400; j < 400; j = j + 15) {
			if (insideObstacle((float)i, (float)j, obstacles, obstacleCount)) {
				continue; // skip calc for inside obstacles
			}
			tempDelta = seekGoal(
				(float)i,
				(float)j,
				//flags[2].location.x,
				//flags[2].location.y,
				bases[3].corners[3].x+15,
				bases[3].corners[3].y-15,
				FLAG_RADIUS,
				&dumpySpeed
			);
			delta.x = tempDelta.x;
			delta.y = tempDelta.y;
			for (oIdx = 0; oIdx < obstacleCount; oIdx++) {
				tempDelta = avoidObstacle(
					(float)i,
					(float)j,
					obstacles[oIdx].center.x,
					obstacles[oIdx].center.y,
					obstacles[oIdx].radius
				);
				delta.x += tempDelta.x;
				delta.y += tempDelta.y;
			}
			for (oIdx = 0; oIdx < obstacleCount; oIdx++) {
				tempDelta = circleObstacle(
					(float)i,
					(float)j,
					obstacles[oIdx].center.x,
					obstacles[oIdx].center.y,
					obstacles[oIdx].radius
				);
				delta.x += tempDelta.x;
				delta.y += tempDelta.y;
			}
			if (delta.x == INFINITY) {
				delta.x = 50;
			}
			if (delta.y == INFINITY) {
				delta.y = 50;
			}
			fprintf(plotFile, "%d %d %02.1f %02.1f\n", i, j, delta.x, delta.y);
			delta.x = 0;
			delta.y = 0;
		}
	}
	free(obstacles);
	free(flags);
	return;
}

int plot() {
	char* gnuplotArgs[4];
	char gnuplotLocation[] = "/usr/bin/gnuplot";
	char gnuplotFile[] = "plot.gpi";
	char gnuplotArg[] = "-persist";
	if (fork() == 0) {
		gnuplotArgs[0] = gnuplotLocation;
		gnuplotArgs[1] = gnuplotArg;
		gnuplotArgs[2] = gnuplotFile;
		gnuplotArgs[3] = 0;
		execv(gnuplotLocation, gnuplotArgs); 
	}
	return 0;
}

pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed) {
	float distance = getDistanceBetween(x, y, xg, yg);
	float angle = getAngleBetween(x, y, xg, yg);
	float speed;
	pair delta;
	if (distance < radiusg) {
		delta.x = 0;
		delta.y = 0;
		speed = 0;
	}
	else if (radiusg <= distance && distance <= (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * (distance - radiusg) * cos(angle);
		delta.y = FLAG_STEP * (distance - radiusg) * sin(angle);
		speed = sqrt(pow(delta.x,2) + pow(delta.y,2));
	}
	else if (distance > (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * FLAG_SPREAD * cos(angle);
		delta.y = FLAG_STEP * FLAG_SPREAD * sin(angle);
		speed = 1.0;
	}
	*retSpeed = speed;
	return delta;
}

pair avoidObstacle(float x, float y, float xo, float yo, float radiuso) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo);
	pair delta;
	delta.x = 0;
	delta.y = 0;

	if (distance < radiuso) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (distance <= (OBSTACLE_SPREAD + radiuso)) {
		delta.x = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiuso - distance) * cos(angle);
		delta.y = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiuso - distance) * sin(angle);
	}
	else if (distance > (OBSTACLE_SPREAD + radiuso)) {
		delta.x = 0;
		delta.y = 0;
	}
	return delta;
}

pair circleObstacle(float x, float y, float xo, float yo, float radiusObs) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo) + 90; 
	pair delta;

	if (distance < radiusObs) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (radiusObs <= distance && distance <= (TANGENT_SPREAD + radiusObs)) {
		delta.x = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * cos(angle);
		delta.y = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * sin(angle);
	}
	else if (distance > (TANGENT_SPREAD + radiusObs)) {
		delta.x = 0;
		delta.y = 0;
	}

	return delta;
}

float getDistanceBetween(float x, float y, float xg, float yg) {
	float distance;
	distance = sqrt(pow((x - xg),2) + pow((y - yg),2));
	return distance;
}

float getAngleBetween(float x, float y, float xg, float yg) {
	float angle;
	angle = atan2((yg - y),(xg - x));
	return angle;
}

int insideObstacle(float x, float y, obstacle* obstacles, int obstacleCount) {
	int i;
	for (i = 0; i < obstacleCount; i++) {
		if (x >= obstacles[i].corners[3].x && x <= obstacles[i].corners[0].x &&
		    y >= obstacles[i].corners[2].y && y <= obstacles[i].corners[0].y) {
			return 1;
		}
	}
	return 0;
}

